#include "math_mid_fltr.h"

#define MID_VAL_FLTR_HALF_WIN_SIZE (MID_VAL_FLTR_WIN_SIZE >> 1)
#define BEFORE_MID 0U
#define AFTER_MID 1U

typedef struct _Node node_t;
typedef struct _Index index_t;

struct _Node {
    float32_t val;
    node_t *nextPtr;
    index_t *indexPtr;
};

struct _Index {
    node_t *nodePtr;
    index_t *prevPtr;
    index_t *nextPtr;
};

typedef struct {
    node_t *headPtr;
    node_t *tailPtr;
} nodes_t;

typedef struct {
    index_t *headPtr;
    index_t *midPtr;
    index_t *tailPtr;
    uint8_t midOffset;
} indexs_t;

__STATIC_INLINE void indexs_insert_after(indexs_t *targtPtr, index_t *prevPtr, index_t *currPtr) {
    
    if (targtPtr->headPtr == 0U) { /* 链表为空 */
        currPtr->prevPtr = 0U;
        currPtr->nextPtr = 0U;
        targtPtr->headPtr = targtPtr->tailPtr = currPtr;
    }else if (prevPtr == 0U) { /* 插入点为链表头 */
        currPtr->nextPtr = targtPtr->headPtr;
        targtPtr->headPtr->prevPtr = currPtr;
        currPtr->prevPtr = 0U;
        targtPtr->headPtr = currPtr;
    }else if (prevPtr == targtPtr->tailPtr) { /* 插入点为链表尾 */
        prevPtr->nextPtr = currPtr;
        currPtr->prevPtr = prevPtr;
        currPtr->nextPtr = 0U;
        targtPtr->tailPtr = currPtr;
    } else { /* 插入点为链表中 */
        prevPtr->nextPtr->prevPtr = currPtr;
        currPtr->nextPtr = prevPtr->nextPtr;
        prevPtr->nextPtr = currPtr;
        currPtr->prevPtr = prevPtr;
    }
}

__STATIC_INLINE void indexs_delete(indexs_t *tgtPtr, index_t *currPtr) {
    
    if (tgtPtr->headPtr == 0U) { /* 链表为空 */
    } else if (currPtr == tgtPtr->headPtr) { /* 删除点为链表头 */
        tgtPtr->headPtr = currPtr->nextPtr;
        tgtPtr->headPtr->prevPtr = 0U;
        tgtPtr->midOffset = BEFORE_MID; 
    } else if (currPtr == tgtPtr->tailPtr) { /* 删除点为链表尾 */
        tgtPtr->tailPtr = currPtr->prevPtr;
        tgtPtr->tailPtr->nextPtr = 0U;
        tgtPtr->midOffset = AFTER_MID; /* 记录中值点偏移 */
    } else { /* 删除点为链表中 */
        if(tgtPtr->midPtr == 0U){ /* 尚无中值信息 */
        }else if(currPtr == tgtPtr->midPtr){ /* 删除点恰为中值点 */
            tgtPtr->midPtr = currPtr->nextPtr;
            tgtPtr->midOffset = AFTER_MID;
        }else if(currPtr->nodePtr->val < tgtPtr->midPtr->nodePtr->val){
            tgtPtr->midOffset = BEFORE_MID;
        }else {
            tgtPtr->midOffset = AFTER_MID;
        }
        currPtr->prevPtr->nextPtr = currPtr->nextPtr;
        currPtr->nextPtr->prevPtr = currPtr->prevPtr;
    }
    currPtr->nextPtr = 0U;
    currPtr->prevPtr = 0U;
}

__STATIC_INLINE void indexs_load_mid_val(indexs_t* targtPtr) {
    
    index_t *currPtr = targtPtr->headPtr;
    uint32_t indexCtr = 0U;
    while (indexCtr < MID_VAL_FLTR_HALF_WIN_SIZE){
        currPtr = currPtr->nextPtr;
        indexCtr++;
    }
    targtPtr->midPtr = currPtr;
}

__STATIC_INLINE void indexs_sort(indexs_t *tgtPtr) {
    
    index_t *currPtr = tgtPtr->headPtr;
    index_t *nextPtr = 0U;
    while (currPtr != tgtPtr->tailPtr) {
        nextPtr = currPtr->nextPtr;
        if (currPtr->nodePtr->val > nextPtr->nodePtr->val) {
            indexs_delete(tgtPtr, currPtr);
            indexs_insert_after(tgtPtr, nextPtr, currPtr);
            if (nextPtr == tgtPtr->headPtr) {
                currPtr = nextPtr;
            } else {
                currPtr = nextPtr->prevPtr;
            }
        } else {
            currPtr = currPtr->nextPtr;
        }
    }
    indexs_load_mid_val(tgtPtr);
}

__STATIC_INLINE void indexs_insert_keep_order(indexs_t *tgtPtr, index_t *currPtr) {
    index_t *ctrPtr = tgtPtr->headPtr;
    while (ctrPtr != 0U) {
        if (currPtr->nodePtr->val > ctrPtr->nodePtr->val) {
            ctrPtr = ctrPtr->nextPtr;
        } else {
            indexs_insert_after(tgtPtr, ctrPtr->prevPtr, currPtr);
            if(tgtPtr->midOffset == BEFORE_MID){
                tgtPtr->midPtr = currPtr->nodePtr->val < tgtPtr->midPtr->nodePtr->val ? tgtPtr->midPtr : tgtPtr->midPtr->nextPtr;
            }
            if(tgtPtr->midOffset == AFTER_MID){
                tgtPtr->midPtr = currPtr->nodePtr->val > tgtPtr->midPtr->nodePtr->val ? tgtPtr->midPtr : tgtPtr->midPtr->prevPtr;
            }
            return;
        }
    }
    indexs_insert_after(tgtPtr, tgtPtr->tailPtr, currPtr);
    tgtPtr->midPtr = tgtPtr->midOffset == AFTER_MID ? tgtPtr->midPtr : tgtPtr->midPtr->nextPtr;
}

__STATIC_INLINE node_t* nodes_out(nodes_t *targtPtr) {
    
    node_t *headPtr = targtPtr->headPtr;
    targtPtr->headPtr = headPtr->nextPtr;
    headPtr->nextPtr = 0;
    return headPtr;
}

__STATIC_INLINE void nodes_in(nodes_t *targtPtr, node_t *tailPtr) {
    
    targtPtr->tailPtr->nextPtr = tailPtr;
    targtPtr->tailPtr = tailPtr;
}

__STATIC_INLINE void load_fltr_val(float32_t *dstPtr, indexs_t *srcPtr, uint32_t valCtr) {
    *(dstPtr + valCtr - MID_VAL_FLTR_HALF_WIN_SIZE) = srcPtr->midPtr->nodePtr->val;
}

__STATIC_INLINE void load_fltr_head(float32_t *dstPtr, nodes_t *srcPtr){
    
    node_t *currPtr = srcPtr->headPtr;
    uint32_t valCtr = 0U;
    while(valCtr < MID_VAL_FLTR_HALF_WIN_SIZE){
        *(dstPtr + valCtr) = currPtr->val;
        currPtr = currPtr->nextPtr;
        valCtr++;
    }
}

__STATIC_INLINE void load_fltr_tail(float32_t *dstPtr, nodes_t *srcPtr, uint32_t beg, uint32_t end){
    
    node_t *currPtr = srcPtr->headPtr;
    uint32_t fltrCtr = 0U;
    while(fltrCtr <= MID_VAL_FLTR_HALF_WIN_SIZE){
        currPtr = currPtr->nextPtr;
        fltrCtr++;
    }
    beg -= MID_VAL_FLTR_HALF_WIN_SIZE; 
    while(beg < end){
        *(dstPtr + beg) = currPtr->val;
        currPtr = currPtr->nextPtr;
        beg++;
    }
}

void mid_val_fltr_usr(float32_t *srcPtr, float32_t *dstPtr, uint32_t len) {
    
    node_t node_arr[MID_VAL_FLTR_WIN_SIZE];
    index_t index_arr[MID_VAL_FLTR_WIN_SIZE];
    nodes_t nodes;
    indexs_t indexs;
    node_t *currNodePtr;
    uint32_t nodeCtr = 0U;

    /* 长度过小无法滤波 */
    if(len < MID_VAL_FLTR_WIN_SIZE)
        return;
    
    /* 初始化节点队列和索引链表 */
    for (; nodeCtr < MID_VAL_FLTR_WIN_SIZE; nodeCtr++) {
        (node_arr + nodeCtr)->val = *(srcPtr + nodeCtr);
        (node_arr + nodeCtr)->nextPtr = nodeCtr < (MID_VAL_FLTR_WIN_SIZE - 1) ? node_arr + nodeCtr + 1 : 0U;
        (node_arr + nodeCtr)->indexPtr = index_arr + nodeCtr;
        (index_arr + nodeCtr)->nodePtr = node_arr + nodeCtr;
        (index_arr + nodeCtr)->prevPtr = nodeCtr > 0 ? index_arr + nodeCtr - 1 : 0U;
        (index_arr + nodeCtr)->nextPtr = nodeCtr < (MID_VAL_FLTR_WIN_SIZE - 1) ? index_arr + nodeCtr + 1 : 0U;
    }
    nodes.headPtr = node_arr;
    nodes.tailPtr = node_arr + MID_VAL_FLTR_WIN_SIZE - 1;
    indexs.headPtr = index_arr;
    indexs.midPtr = 0U;
    indexs.tailPtr = index_arr + MID_VAL_FLTR_WIN_SIZE - 1;
    /* 初始化结束 */

    
    /* 开始中间值滤波 */
    load_fltr_head(dstPtr, &nodes); /* 将数据起始处不足滤波个数的点装载到指定的储存区 */
    indexs_sort(&indexs); /* 对无序索引链表进行排序 */
    load_fltr_val(dstPtr, &indexs, nodeCtr - 1); /* 将结果装载到指定的储存区 */
    
    for (; nodeCtr < len; nodeCtr++) {
        currNodePtr = nodes_out(&nodes); /* 弹出队头节点 */
        indexs_delete(&indexs, currNodePtr->indexPtr); /* 弹出节点索引 */
        currNodePtr->val = *(srcPtr + nodeCtr); /* 重新装载新值入节点 */
        nodes_in(&nodes, currNodePtr); /* 节点插入对尾 */
        indexs_insert_keep_order(&indexs, currNodePtr->indexPtr); /* 节点索引插入有序链表 */
        load_fltr_val(dstPtr, &indexs, nodeCtr); /* 将结果装载到指定的储存区 */
    }
    load_fltr_tail(dstPtr, &nodes, nodeCtr, len); /* 将数据结尾处不足滤波个数的点装载到指定的储存区 */
    /* 中间值滤波完成 */
}
